function p= least_square_old(dis,n)
%最小二乘（旧）
%输入四个距离,n组，输出位置
p=zeros(n,3);
x0=0;y0=0;z0=0;
x1=x0+0.95;
y1=y0;
z1=z0;
x2=x0;
y2=y0+0.7;
z2=z0;
x3=x0+0.95;
y3=y0+0.7;
z3=z0;
for i=1:3541
    d0=dis(i,1);
    d1=dis(i,2);
    d2=dis(i,3);
    d3=dis(i,4);
    A=[2*(x1-x0),2*(y1-y0);
       2*(x2-x0),2*(y2-y0);
       2*(x3-x0),2*(y3-y0)];
    B=[x1^2-x0^2+y1^2-y0^2+d0^2-d1^2;
       x2^2-x0^2+y2^2-y0^2+d0^2-d2^2;
       x3^2-x0^2+y3^2-y0^2+d0^2-d3^2];
    X=(A'*A)^(-1)*A'*B;
    xx=X(1);
    yy=X(2);
    zz2=(d0^2+d1^2+d2^2+d3^2-(xx-x0)^2-(yy-y0)^2-(xx-x1)^2-(yy-y1)^2-(xx-x1)^2-(yy-y2)^2-(xx-x3)^2-(yy-y3)^2)/4;
    zz=sqrt(abs(zz2));
    p(i,:)=[xx,yy,zz];
end
end

